#define F_CPU 1000000L
#include <avr/io.h>
#include <util/delay.h>
#define USART_BAUD 2400
#define USART_UBBR_VALUE ((F_CPU/(USART_BAUD<<4))-1)

void setAngle(int angle);
void InitServ(void);

void USART_Init();
void USART_Transmit(char data);
void USART_TransmitString(char *str);
uint8_t uart_ischar();
uint8_t uart_getc();

int main(void) {
	uint8_t c;

	USART_Init();
	while(1)
	{
		c = uart_getc();
		//USART_Transmit('\n');
		USART_Transmit(c);
	}
}

void setAngle(int angle) {
        PORTB |= (1 << PB1);
        OCR1A = angle;
}

void USART_Init() {
{
// Set baud rate
    UBRRH = (unsigned char)(USART_UBBR_VALUE>>8);
    UBRRL = (unsigned char)USART_UBBR_VALUE;

// Enable receiver and transmitter
    UCSRB = (1<<RXEN)|(1<<TXEN);

// Set frame format to 8 data bits, no parity, 1 stop bit
    UCSRC = (0<<USBS)|(1<<UCSZ1)|(1<<UCSZ0)|(0<<UCSZ2)|(1<<URSEL);


}
}

void USART_Transmit(char data) {

        /* Wait for empty transmit buffer */
        while (!(UCSRA & (1 << UDRE)))
                ;
        /* Put data into buffer, sends the data */
        UDR = data;
}

void USART_TransmitString(char *str) {
        int i = 0;
        for (i = 0; str[i] != '\0'; i++) {
                USART_Transmit(str[i]);
                _delay_ms(50);
        }
}

void InitServ(void) {
        DDRB |= (1 << PB1) | (1 << PB2);
        PORTB = 0;

        ICR1 = 20000;
        OCR1A = 1000;

        TCCR1A |= (0 << COM1A0) | (1 << COM1A1) | (0 << COM1B0) | (0 << COM1B1)
                        | (0 << FOC1A) | (0 << FOC1B) | (1 << WGM11) | (0 << WGM10);

        TCCR1B |= (0 << ICNC1) | (0 << ICES1) | (1 << WGM13) | (1 << WGM12)
                        | (0 << CS12) | (0 << CS11) | (1 << CS10);
}

uint8_t uart_ischar()
{
	// Czy w buforze sš dane?
	return (UCSRA & (1 << RXC));
}

uint8_t uart_getc()
{
	// Czy w buforze sš dane?
	while(!uart_ischar());

	// Dane z bufora
	return UDR;
}
